#include "chassic.hpp"

#include <fstream>
#include <iomanip>
#include <iostream>

#include "gtest/gtest.h"
#include "lidar.hpp"
#include "log.hpp"
#include "spdlog/spdlog.h"

// #ifdef WHEEL_TEC

// TEST(TestDevice, TestRobotRecv) {
//   Chassic chassic;
//   chassic.Init("/dev/ttyACM0");
//   while (true) {
//     SPDLOG_WARN("vx  : {}", mcu);
//   }
//   EXPECT_EQ(1, 1);
// }

// #else

// // TEST(TestDevice, TestRobotRecv) {
// //   component::Logger::SetLogger("logs/log.log");
// //   Chassic chassic;
// //   chassic.Init("/dev/ttyACM0");
// //   while (true) {
// //     auto mcu = chassic.GetMCU();
// //     SPDLOG_WARN(
// //         "\nimu :\n"
// //         "    accl:\t\tax : {} \t\tay : {} \t\taz : {} \n"
// //         "    gyro:\t\twx : {} \t\twy : {}  \t\twz : {} \n"
// //         "    ahrs:\t\tpit : {} \t\trol : {} \t\tyaw : {} \n"
// //         "motor : \n\t {}  {}  {}  {}",
// //         (float)mcu.imu.accl.ax, (float)mcu.imu.accl.ay,
// //         (float)mcu.imu.accl.ay, (float)mcu.imu.gyro.wx,
// //         (float)mcu.imu.gyro.wy, (float)mcu.imu.gyro.wz,
// //         (float)mcu.imu.ahrs.pit, (float)mcu.imu.ahrs.rol,
// //         (float)mcu.imu.ahrs.yaw, (float)mcu.motors.m1,
// (float)mcu.motors.m2,
// //         (float)mcu.motors.m3, (float)mcu.motors.m4);
// //   }
// //   EXPECT_EQ(1, 1);
// // }

// TEST(TestDevice, TestRobotTrans) {
//   component::Logger::SetLogger("logs/trans.log");
//   Chassic chassic;
//   chassic.Init("/dev/ttyACM0");
//   while (true) {
//     for (int i = 1; i <= 10; i++) {
//       SPDLOG_WARN("{}", i);
//       chassic.Pack(component::Velocity(i, i, 1));
//     }
//   }
//   EXPECT_EQ(1, 1);
// }

// #endif

TEST(TestDevice, TestRobotAndr) {
  component::Logger::SetLogger("logs/trans.log");
  Android chassic;
  chassic.Init("/dev/ttyTHS1");
  LidarDriver lidar;
  lidar.Init();
  int i = 0;
  while (true) {
    lidar.Apply();
    auto p = lidar.GetResult();
    chassic.Pack(p);
    SPDLOG_WARN("i, {}", ++i);
  }

  EXPECT_EQ(1, 1);
}